// Astar.cpp: implementation of the Astar class.
//
//////////////////////////////////////////////////////////////////////

#include "Astar.h"



//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

Astar::Astar()
{

}

Astar::~Astar()
{
	Reset();
}


void Astar::Reset()
{
	myOpenList.clear();
	myCloseList.clear();

	while(!myPQueue.empty())
	   myPQueue.pop();
}

void Astar::PrintAndReleasePQueue()
{
	NodeState current;
	while(!myPQueue.empty())
	{
		current = myPQueue.top();
		current.Print();
		myPQueue.pop();
	}
}



void Astar::AddStateToOpenList(I64 hashVal, int gVal, int hVal)
{
	int val;
	val = gVal;
	hVal = hVal<<8;
	val = hVal | gVal;

	myOpenList[hashVal] = val;
}

bool Astar::IsPQueueEmpty()
{
	return myPQueue.empty();
}

void Astar::AddStateToCloseList(I64 hashVal, int gVal, int hVal)
{

	int val;
	val = gVal;
	hVal = hVal<<8;
	val = hVal | gVal;

	myCloseList[hashVal] = val;
}

bool Astar::GetStateFromCloseList(I64 hashVal, unsigned char &gVal, unsigned char &hVal)
{
	int val;

	if (myCloseList.find(hashVal) == myCloseList.end())
	{
		return false; // cant locate state in close list
	}

	val =  myCloseList[hashVal];
	
	gVal = (unsigned char)(val & 255);
	hVal = (unsigned char)(val>>8);
	return true;
}

bool Astar::GetStateFromOpenList(I64 hashVal, unsigned char &gVal, unsigned char &hVal)
{
	int val;

	if (myOpenList.find(hashVal) == myOpenList.end())
	{
		return false; // cant locate state in close list
	}

	val =  myOpenList[hashVal];
	
	gVal = (unsigned char)(val & 255);
	hVal = (unsigned char)(val>>8);
	return true;
}

void Astar::ResetMemory()
{
	myOpenList.clear();
	myCloseList.clear();

	while(!myPQueue.empty())
	   myPQueue.pop();

}





/******************************************************************************
 * Function Name:
 * Input: NodeState * root - the root
 *        hashFunc
 * 
 ******************************************************************************/

 int Astar::AStarSearch(NodeState root,
		   unsigned char (*hcalc) (NodeState), 
		   bool (*IsItGoal) (NodeState),
		   NodeState * (*children) (NodeState,int &),
		   I64 (*hashCalc)(NodeState), bool use_cut_off, I64 &generatedNumber
		   , I64 &expendedNumber, bool UpdateFromListsCut)
{
	generatedNumber = 0;
	expendedNumber = 0;
	unsigned char GcurChild,HcurChild;
	int nChild,i ;
	bool boo;
	unsigned char maxH;
	NodeState *childList;
	NodeState bestNode;
	ResetMemory();


	I64 localCounter = 0;
	I64 currentHash;

	/*while(1) //for testing
	{
		localCounter++;
		myPQueue.push(root); //push first state
		AddStateToOpenList(localCounter ,(int)root.gValue ,(int)root.hValue);
		printf("%d\t%d\t \r",myPQueue.size(),myOpenList.size());
	}*/


//	NodeState tmp;
//	find(root,tmp,myPQueue);

	myPQueue.push(root); //push first state
	AddStateToOpenList(hashCalc(root) ,(int)root.gValue ,(int)root.hValue);

	while (!myPQueue.empty())
	{
		// remove the first node from the PQueue , as it will always be sorted
		bestNode = myPQueue.top();
		myPQueue.pop();
		currentHash = hashCalc(bestNode);
		myOpenList.erase(currentHash);

		// is the current node the goal node?
		if (bestNode.hValue == 0 && IsItGoal(bestNode))
		{
			ResetMemory();
			return bestNode.gValue;
		}

		if (GetStateFromCloseList(currentHash, GcurChild, HcurChild))
		{
			if (GcurChild  <= bestNode.gValue )
			{//already been in this state...
				myOpenList.erase(currentHash);
				continue;
			}
		}
		
		//generate children
		childList = children(bestNode, nChild);
		generatedNumber+=nChild;
		expendedNumber++;

		if ( generatedNumber >  MAXNODES)
		{//failed - no memory...
			delete[] childList;
			ResetMemory();
			exit(0);
		}

		localCounter++;
		if (localCounter == 10000) 
		{
			printf("%I64d\t%d\t%d\t%d\t%d\t\t \r",generatedNumber, bestNode.F(),myCloseList.size(), myOpenList.size(), myPQueue.size());
			localCounter = 0;
		}

		for (i = 0 ; i < nChild ; i++)
		{
			if (childList[i].hValue == 0) //else was already calculated
				childList[i].hValue = hcalc(childList[i]);
		}
		
		bool bShouldPushtoTpQueue[SIZE];
		if (use_cut_off)
		{
			maxH = 0;
			for (i = 0 ; i < nChild ; i++)
				maxH = max(maxH , childList[i].hValue - 2);
			for (i = 0 ; i < nChild ; i++)
			{
				childList[i].hValue = max(maxH , childList[i].hValue);
				bShouldPushtoTpQueue[i] = false;
			}
		}


		// insert the children into the PQueue list (and to the open list) according to thier f values
		for (i = 0 ; i < nChild ; i++)
		{
			childList[i].gValue = bestNode.gValue + 1;

			if (use_cut_off)
			{
				if (childList[i].F() < bestNode.F())
				{
					childList[i].hValue = bestNode.F() - childList[i].gValue ;
				}
			}

			if (use_cut_off)
				childList[i].hValue  = max(childList[i].hValue , maxH - 2);
			boo = true;

			// test whether the child is in the closed list (already been there)
			if (GetStateFromCloseList(hashCalc(childList[i]) , GcurChild, HcurChild))
			{

				if (use_cut_off && UpdateFromListsCut)
				{
					if (HcurChild > childList[i].hValue)
					{
						childList[i].hValue = HcurChild;
					}
				}

				// if so, check if the g value is lower
				if (GcurChild  <= childList[i].gValue) 
				{	 
					// if the f value of the older node is lower, delete the new
					// child (dont add it to pQueue)
					boo = false;
				}
				else
				{   // the child is a shorter path to this point, delete p from
					// the closed list
					
					myCloseList.erase(hashCalc(childList[i]));
					boo = true;
				}
			}

			if (boo)
			{
				// check if the child is already on the open list
				if (GetStateFromOpenList(hashCalc(childList[i]) , GcurChild, HcurChild))
				{
						if (use_cut_off && UpdateFromListsCut)
						{
							if (HcurChild > childList[i].hValue)
							{
								childList[i].hValue = HcurChild;
							}
						}
					  // child is on the OPEN list
						if ((GcurChild ) <= childList[i].gValue ) 
						{   // child is a longer path to the same place so delete it
							boo = false;
						}
						else 
						{		// child is a shorter path to the same place
								// remove the duplicate node
							myOpenList.erase(hashCalc(childList[i]));
							boo = true;
						}
				}
			}

			if (boo) 
			{
				// now insert the child into the pQueue and to the open list 
				// according to the f values
				if (use_cut_off == false)
				{
					myPQueue.push(childList[i]); 
					AddStateToOpenList(hashCalc(childList[i]) ,(int)childList[i].gValue ,(int)childList[i].hValue);
				}
				else
					bShouldPushtoTpQueue[i] = true;
			}
		}

		if (use_cut_off)
		{
			maxH = 0;
			for (i = 0 ; i < nChild ; i++)
				maxH = max(maxH , childList[i].hValue - 2);

			for (i = 0 ; i < nChild ; i++)
			{
				if (bShouldPushtoTpQueue[i])
				{
					childList[i].hValue = (int)max(maxH,(int)childList[i].hValue);
					myPQueue.push(childList[i]); 
					AddStateToOpenList(hashCalc(childList[i]) ,(int)childList[i].gValue ,(int)childList[i].hValue);
				}
			}
		}

		// put the current node onto the closed list
		AddStateToCloseList(hashCalc(bestNode) ,(int)bestNode.gValue ,(int)bestNode.hValue);

		// Test to see if we have expanded too many nodes without a solution

		delete[] childList;
	}

	return -1;
}

